{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 02-01 模型的导入和查看\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "### laikago模型\n",
    "\n",
    "宇树机器人（unitree robotics）在github上的官方仓库中，提供了unitree_pybullet包。\n",
    "\n",
    "其中给出了laikago、aliengo、a1三个机器人的模型。\n",
    "这里选择laikago的模型，将其拷贝到models文件夹中使用。\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 使用MeshCat显示laikago的三维模型\n",
    "\n",
    "使用可视化工具可直观查看模型。\n",
    "这里用MeshCat。因为Drake的很多例子中都是用MeshCat。\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "server_args = []\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Determine if this notebook is currently running as a notebook or a unit test.\n",
    "from IPython import get_ipython\n",
    "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Imports\n",
    "import numpy as np\n",
    "import pydot\n",
    "from ipywidgets import Dropdown, Layout\n",
    "from IPython.display import display, HTML, SVG\n",
    "\n",
    "from pydrake.all import (\n",
    "    AddMultibodyPlantSceneGraph, ConnectMeshcatVisualizer, DiagramBuilder, \n",
    "    FindResourceOrThrow, GenerateHtml, InverseDynamicsController, \n",
    "    MultibodyPlant, Parser, Simulator)\n",
    "from pydrake.multibody.jupyter_widgets import MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Connecting to meshcat-server at zmq_url=tcp://127.0.0.1:6000...\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n",
      "Connected to meshcat-server.\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f5c72b7c658b4414a24c2e920b35f8db",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FR_hip_joint', layout=Layout(width=\"'200'\"), max=0.872664625997, min=-1.04…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "21e71afc68fa4b19bfdf41575009c320",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FR_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bdda40fccc07407088a0dacfd947d92c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='FR_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "7cef4436eb8d4339922015f106d90982",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FL_hip_joint', layout=Layout(width=\"'200'\"), max=1.0471975512, min=-0.8726…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "70f59e64330d4ae3ad0829175df376ad",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='FL_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "89ada802f20c420093d45e5ab5461587",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='FL_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "089ff374a16945d7ac32aa3d13cd159e",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RR_hip_joint', layout=Layout(width=\"'200'\"), max=0.872664625997, min=-1.04…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "f89da830fecf4da8a356afad246a850b",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RR_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "557ac7eff4424c318359906c4da4e5e8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='RR_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a554763183664532be263984c2079e14",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RL_hip_joint', layout=Layout(width=\"'200'\"), max=1.0471975512, min=-0.8726…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "a78cfcc6a49842ba92c8dbcdeb77338c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=0.0, description='RL_thigh_joint', layout=Layout(width=\"'200'\"), max=3.92699081699, min=-0.5…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "382a5f84aff244dfbaf872bed592b064",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "FloatSlider(value=-0.610865238198, description='RL_calf_joint', layout=Layout(width=\"'200'\"), max=-0.610865238…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#import sys\n",
    "#sys.path.append('/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/')\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)\n",
    "\n",
    "Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/laikago/urdf/laikago1_obj.urdf\")\n",
    "#Parser(plant, scene_graph).AddModelFromFile(\"/mnt/c/Users/ahl19/Documents/my_gitRepos/controller4quadruped/models/laikago/urdf/laikago1.urdf\")\n",
    "\n",
    "\n",
    "\n",
    "plant.Finalize()\n",
    "\n",
    "meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url)\n",
    "diagram = builder.Build()\n",
    "context = diagram.CreateDefaultContext()\n",
    "\n",
    "meshcat.load()\n",
    "MakeJointSlidersThatPublishOnCallback(plant, meshcat, context);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "上面的导入中，惊奇的发现MeshCat还仅（官方）支持obj文件的模型。\n",
    "\n",
    "而unitree官方给的模型只有stl和dae模型文件。\n",
    "经过查找，在pybullet软件包给出的模型中，找到了带有obj模型文件的laikago模型。\n",
    "不过导入仍有问题。\n",
    "\n",
    "最近的一次尝试是用在线转换工具，autoconvert网站，将stl格式的模型文件转换为obj格式的模型文件。\n",
    "运行meshcat.load()是可以的（有了obj模型文件后，在urdf中无论使用stl或obj都可以）。\n",
    "但是在meshcat环境中只能看到机器人的四个足端（无论使用stl或obj都可以，都看不到其他部分。。）\n",
    "\n",
    "在多次尝试没有成功的情况下，放弃在MeshCat中查看的目标。\n",
    "改为在CoppeliaSim中进行查看。\n",
    "【注意】在coppelia和meshcat中查找模型文件的方式不同。coppelia（win）可以使用package，但meshcat需要用../\n",
    "\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
